import math as m

import sys

import cv2
import numpy as np
import sympy as sp

# from rich import print

from tma.utils import settings as s
from tma.utils.custom_types import t_line, t_lines, t_point

from tma.utils.bfgs import predict as bfgs_predict
from tma.utils.cache_fun_05 import evaluate_track_05, cal_track_info_05
from tma.utils.cache_fun_06 import predict_06
from tma.utils.cache_fun import (
    cal_track_fitness_metric,
    evaluate_track,
    find_points_on_line_by_distance,
    cal_track_info,
)
from tma.utils.tools import *


class TargetPredction:
    def __init__(self):
        self.track: t_line = []
        self.track_error: float = 9999
        self.track_heading: tuple[float, float] = tuple()
        self.track_distance: float = 0
        self.track_detected: bool = False
        self.top3_track: dict[float, list[float]] = {}


class Target:
    def __init__(
        self,
        head: int = 0,
        tail: int = -1,
        # direction: TgtDirection,
        track_leg: int = 1,
    ) -> None:
        self.head = head
        self.tail = tail
        # self.direction: TgtDirection = direction
        self.track_leg = track_leg

        # self.track_error: float = 0
        # self.best_track: t_line = []
        # self.top3_track: dict[float, list[float]] = {}
        # self.best_track_heading: tuple[float, float] = tuple()
        # self.estimated_travel_distance: float = 0
        # self.track_detected: bool = False
        # self.track_distance_detected: float = travel_distance  # 325.8619339536301

        self.pd_with_track: TargetPredction = TargetPredction()
        self.pd_without_track: TargetPredction = TargetPredction()
        self.ekelund_range: list = []


class TMA:
    def __init__(self, target: Target, image_file: str = "", bl: t_lines = None) -> None:
        self.target = target

        self.original_lines: t_lines = []
        if bool(image_file):
            self.read_image(image_file)
        elif bool(bl):
            self.lines = bl
            self.bl_time_tick = list(range(len(bl)))
            all_boundary_points = []
            for line in self.lines:
                all_boundary_points.append(sp.Point(line[:2], evaluate=False))
                all_boundary_points.append(sp.Point(line[2:], evaluate=False))

            self.view_center: t_point = sp.centroid(*all_boundary_points).coordinates
            self.average_slope = cal_average_slope(self.lines)
            slope = -1 / self.average_slope
            x_vc, y_vc = self.view_center
            group_cross_line = [x_vc, y_vc, x_vc + 50, y_vc + slope * 50]
            self.view_center_group_cross_line = group_cross_line
            self.update_bl_related_info()
        else:
            raise Exception("Unable to initialize TMA")

        self.boundary: t_lines = [self.lines[0], self.lines[-1]]

    def find_target_travel_distance(self, image):
        """
        must be called after self.normalize_line_data()
        estimate target track length
        if traget track detected, return it's length
        """

        # !!@ retrieve #f80101 : akula class
        # !!@ #f80101 is similar to bearing line color, must use following method
        lower_range = np.array([0, 0, 50])
        upper_range = np.array([25, 50, 255])
        mask = cv2.inRange(image, lower_range, upper_range)
        masked_image = cv2.bitwise_and(image, image, mask=mask)

        # cv2.imshow("Extracted Color Range", masked_image)
        # cv2.waitKey(0)
        # cv2.destroyAllWindows()

        gray = cv2.cvtColor(masked_image, cv2.COLOR_BGR2GRAY)
        # blur = cv2.GaussianBlur(gray, (5, 5), 0)
        # edges = cv2.Canny(blur, 5, 15, apertureSize=3)

        detected_lines = cv2.HoughLinesP(
            gray,
            1,
            np.pi / s.HoughLinesP_precise_1,
            threshold=s.HoughLinesP_threshold,
            minLineLength=s.HoughLinesP_minLineLength_1,
            maxLineGap=s.HoughLinesP_maxLineGap,
        )

        # !!@ retrieve #00fb00 : virginia class
        if detected_lines is None:
            # Convert the image from BGR to RGB
            image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
            # Define the color you want to match (here we use #00fb00)
            target_color = np.array([0, 251, 0])  # BGR values for #00fb00
            # Calculate the color similarity threshold (you can adjust this value)
            color_threshold = 150
            # Calculate the Euclidean distance between each pixel and the target color
            distances = np.linalg.norm(image_rgb - target_color, axis=2)
            # Create a mask based on color similarity threshold
            mask = (distances < color_threshold).astype(np.uint8) * 255
            # Apply the mask to the original image
            masked_image = cv2.bitwise_and(image, image, mask=mask)

            gray = cv2.cvtColor(masked_image, cv2.COLOR_BGR2GRAY)
            # blur = cv2.GaussianBlur(gray, (5, 5), 0)
            # edges = cv2.Canny(blur, 5, 15, apertureSize=3)

            # cv2.imshow("Extracted Color Range", masked_image)
            # cv2.waitKey(0)
            # cv2.destroyAllWindows()

            detected_lines = cv2.HoughLinesP(
                gray,
                1,
                np.pi / s.HoughLinesP_precise_1,
                threshold=s.HoughLinesP_threshold,
                minLineLength=s.HoughLinesP_minLineLength_1,
                maxLineGap=s.HoughLinesP_maxLineGap,
            )

        # !!@ retrieve #01ef01 : Los class
        if detected_lines is None:
            # Convert BGR to HSV
            hsv_image = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
            # #01ef01
            # Define the lower and upper bounds of the green color in HSV
            lower_green = np.array([40, 50, 50])
            upper_green = np.array([80, 255, 255])
            # Create a mask to keep only green colors
            mask = cv2.inRange(hsv_image, lower_green, upper_green)
            # gray = cv2.cvtColor(masked_image, cv2.COLOR_BGR2GRAY)
            # blur = cv2.GaussianBlur(gray, (5, 5), 0)
            # edges = cv2.Canny(blur, 5, 15, apertureSize=3)

            # cv2.imshow("Extracted Color Range", mask)
            # cv2.waitKey(0)
            # cv2.destroyAllWindows()

            detected_lines = cv2.HoughLinesP(
                mask,
                1,
                np.pi / s.HoughLinesP_precise_1,
                threshold=s.HoughLinesP_threshold,
                minLineLength=s.HoughLinesP_minLineLength_1,
                maxLineGap=s.HoughLinesP_maxLineGap,
            )

        lines: t_lines = []

        # print("1" * 111)
        # print(detected_lines)
        # print("2" * 111)

        if detected_lines is not None:
            for line in detected_lines:
                lines.append(list(line[0]))
            self.target.pd_with_track.track_distance = (
                max([distance_between_points(line[:2], line[2:]) for line in lines]) - s.Target_track_arrow
            )
            # print(detected_lines)
            # print("1" * 11)
            # print(lines)
            # print(self.target.track)
            # print(self.normalized_lines)
            # sys.exit(1)
            self.target.pd_with_track.track_detected = True
        # else:
        #     self.target.pd_without_track.track_distance = self.view_center_cross_distance

    def normalize_line_data(self):
        """replace every line with intersection_point of image boundary
        sorted lines
        merged similar lines
        self.lines
        self.bl_time_tick
        """
        # y = m*x + b
        unsort_lines: t_lines = []

        self.average_slope = cal_average_slope(self.lines_switched_coordinate)
        # !!@ extend all lines to reach image-box
        for idx, line in enumerate(self.lines_switched_coordinate):
            x1, y1, x2, y2 = line
            x1, y1, x2, y2 = float(x1), float(y1), float(x2), float(y2)
            unsort_lines.append([])
            if x1 == x2:
                unsort_lines[idx] = [x1, 0, x1, self.canvas_size[0]]
                continue
            if y1 == y2:
                unsort_lines[idx] = [0, y1, self.canvas_size[1], y1]
                continue
            for box_line in self.image_box:
                x, y = find_intersection_point(line, box_line)
                if x < 0 or x > self.canvas_size[1] or y < 0 or y > self.canvas_size[0]:
                    continue
                unsort_lines[idx].extend([x, y])
            assert len(unsort_lines[idx]) == 4

        all_boundary_points = []
        for line in unsort_lines:
            all_boundary_points.append(sp.Point(line[:2], evaluate=False))
            all_boundary_points.append(sp.Point(line[2:], evaluate=False))

        self.view_center: t_point = sp.centroid(*all_boundary_points).coordinates

        # @TODO no more bl_intersection_center
        # self.bl_intersection_center = cal_intersection_center(unsort_lines)

        # @TODO cornor case: slope == 0, group_cross_line is vertical to x-axis
        slope = -1 / self.average_slope
        # y = m*x + b
        x_vc, y_vc = self.view_center
        group_cross_line = [x_vc, y_vc, x_vc + 50, y_vc + slope * 50]
        self.view_center_group_cross_line = group_cross_line

        # !!@ sort lines
        # some lines are very close to each other, them may cross !!!
        cross_points = [(find_intersection_point(group_cross_line, i), index) for index, i in enumerate(unsort_lines)]
        ordered_index = [i[1] for i in sorted(cross_points, key=lambda item: item[0][0])]
        ordered_lines = [unsort_lines[i] for i in ordered_index]
        self.normalized_lines = ordered_lines
        # self.view_center_cross_distance = distance_between_points(
        #     find_intersection_point(ordered_lines[0], group_cross_line),
        #     find_intersection_point(ordered_lines[-1], group_cross_line),
        # )

        # !!@ merge similar lines
        line_merger = HoughBundler(2, min_angle=0.4)
        self.lines = line_merger.process_lines(self.normalized_lines)
        self.lines = line_merger.process_lines(self.lines)

        # !!@ sort merged lines
        # after this, we can guarantee all merged lines are sorted
        cross_points = [(find_intersection_point(group_cross_line, i), index) for index, i in enumerate(self.lines)]
        ordered_index = [i[1] for i in sorted(cross_points, key=lambda item: item[0][0])]
        ordered_lines = [self.lines[i] for i in ordered_index]

        # !!@ resolve a corner case for HoughBundler
        # (line1: 45.2 line2: 49.2 will be assigned to different groups)
        final_merge_lines = []
        for index, line in enumerate(ordered_lines):
            if index == 0:
                final_merge_lines.append(line)
                continue
            if get_distance_between_lines(ordered_lines[index], final_merge_lines[-1]) < 5:
                pre_line = final_merge_lines[-1]
                new_line = [
                    (pre_line[0] + line[0]) / 2,
                    (pre_line[1] + line[1]) / 2,
                    (pre_line[2] + line[2]) / 2,
                    (pre_line[3] + line[3]) / 2,
                ]
                if distance_between_points(new_line[:2], new_line[2:]) < 30:
                    final_merge_lines[-1] = [
                        (pre_line[0] + line[2]) / 2,
                        (pre_line[1] + line[3]) / 2,
                        (pre_line[2] + line[0]) / 2,
                        (pre_line[3] + line[1]) / 2,
                    ]
                else:
                    final_merge_lines[-1] = new_line
                t = final_merge_lines[-1]
                assert distance_between_points(t[:2], t[2:]) > 30
            else:
                final_merge_lines.append(line)

        # !!@ ensure line slope of self.lines[0] > [-1]
        if abs(get_line_slope_and_intercept(final_merge_lines[0])[0]) > abs(
            get_line_slope_and_intercept(final_merge_lines[-1])[0]
        ):
            final_merge_lines.reverse()
        self.lines = final_merge_lines
        self.bl_time_tick = list(range(len(final_merge_lines)))

        # !!@ image size used for echart x-axis/y-axis
        max_x = 0
        max_y = 0
        min_x = 1000
        min_y = 1000
        for line in self.lines:
            if max_x < max(line[0], line[2]):
                max_x = max(line[0], line[2])
            if max_y < max(line[1], line[3]):
                max_y = max(line[1], line[3])
            if min_x > min(line[0], line[2]):
                min_x = min(line[0], line[2])
            if min_y > min(line[1], line[3]):
                min_y = min(line[1], line[3])
        self.image_size = [min_x, min_y, max_x, max_y]

    def update_bl_related_info(self):
        """
        generate own postion points, polulate bl orientation list
        self.own_pos
        self.bl_feature
        self.bl_headings
        self.bl_slopes
        self.bl_feature
        self.bl_orientation
        self.bl_change_ratio
        """
        # !!@ generate own positions
        outer_lines_cross_p = find_intersection_point(self.lines[0], self.lines[-1])
        if not outer_lines_cross_p:
            raise Exception("First and last bearring seems parallel")
        if self.view_center[0] <= outer_lines_cross_p[0]:
            x0 = outer_lines_cross_p[0] + 500
        else:
            x0 = outer_lines_cross_p[0] - 500
        average_bl = tuple(self.view_center) + tuple(outer_lines_cross_p)
        x1, y1 = find_vertical_point(average_bl, (x0, 0))
        self.own_pos: tuple[t_point, ...] = tuple(find_intersection_point([x0, 0, x1, y1], i) for i in self.lines)

        # print("-" * 88)
        # print(self.own_pos)
        # assert is_ordered([i[0] for i in self.own_pos]) is True

        # !!@ bl_orientation / bl_change_ratio
        self.bl_feature = if_bl_spread_over_y_axis(self.lines)
        self.bl_headings = [sorted(cal_line_angle(line)) for line in self.lines]
        slopes = []
        for line in self.lines:
            x1, y1, x2, y2 = line
            if x2 == x1:
                slopes.append(sys.maxsize - 10)
            else:
                slopes.append((y2 - y1) / (x2 - x1))
        self.bl_slopes = slopes

        if self.bl_feature[1] != 0:
            bl_orientation = [min(i) for i in self.bl_headings]
        else:
            if self.bl_feature[0]:
                bl_orientation = []
                for index, hd in enumerate(self.bl_headings):
                    if self.bl_slopes[index] >= 0:
                        bl_orientation.append(max(hd))
                    else:
                        bl_orientation.append(min(hd))
            else:
                bl_orientation = [min(i) for i in self.bl_headings]
        self.bl_orientation = bl_orientation

        self.calculate_bl_change_ratio_polyfit()

        assert len(self.bl_change_ratio) == len(self.lines)

    def single_leg_ekelund_ranging(self):
        self.target.ekelund_range = []
        for _, ratio in enumerate(self.bl_change_ratio):
            self.target.ekelund_range.append({
                j: round(abs(j / ratio), 1)
                for j in range(
                    s.Ekelund_range_spd_btm,
                    s.Ekelund_range_spd_top,
                    s.Ekelund_range_spd_step,
                )
            })
        # print("+" * 44)
        # print("compare to other auto-tma method")
        # print(self.lines)
        # print(self.bl_time_tick)
        # print(self.bl_orientation)

    def calculate_bl_change_ratio_polyfit(self):
        """
        use np.poly1d/np.polyfit to adjust target bearing line change ratio
        self.bl_change_ratio
        """
        bl_orientation = self.bl_orientation

        x_data = [float(i) for i, _ in enumerate(bl_orientation)]
        y_data = bl_orientation
        best_dgr = get_best_adjR(x_data, y_data)
        best_fit_fn = np.poly1d(np.polyfit(x_data, y_data, best_dgr))
        best_fit_derivative_fn = np.polyder(best_fit_fn)

        self.bl_change_ratio = [float(best_fit_derivative_fn(i)) for i in range(0, len(bl_orientation))]
        assert len(self.bl_change_ratio) == len(self.lines)

    def read_image(self, image_file: str):
        """read TMA station screenshot
        self.original_lines
        self.lines_switched_coordinate
        self.lines_with_mask
        self.display_lines
        """
        image = cv2.imread(image_file)

        # !!@ mask left-up cornor
        mask_area = (
            0,
            1,
            m.ceil(s.Image_maskarea_ratio.w * image.shape[1]),
            m.ceil(s.Image_maskarea_ratio.h * image.shape[0]),
        )
        # rectangle coordinates (x, y, width, height)
        x, y, w, h = mask_area
        image[y : y + h, x : x + w] = [0, 0, 0]

        image_copy = image.copy()

        # @TODO restrict image.shape (500, 630, 3)

        # (height, width)
        self.canvas_size = (image.shape[0], image.shape[1])
        self.image_box = (
            (0, 0, 0, self.canvas_size[0]),
            (0, self.canvas_size[0], self.canvas_size[1], self.canvas_size[0]),
            (self.canvas_size[1], self.canvas_size[0], self.canvas_size[1], 0),
            (self.canvas_size[1], 0, 0, 0),
        )

        # !!@ method 1 retrieve bearing lines
        # image = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        # lower_range = np.array([56, 0, 92])
        # upper_range = np.array([179, 255, 255])
        # masked_image = cv2.inRange(image, lower_range, upper_range)

        # !!@ method 2 retrieve bearing lines
        image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
        # Define the color you want to match (here we use #b300b3)
        target_color = np.array([179, 0, 179])  # BGR values for #b300b3
        # Calculate the color similarity threshold (you can adjust this value)
        color_threshold = 150
        # Calculate the Euclidean distance between each pixel and the target color
        distances = np.linalg.norm(image_rgb - target_color, axis=2)
        # Create a mask based on color similarity threshold
        mask = (distances < color_threshold).astype(np.uint8) * 255
        # Apply the mask to the original image
        masked_image = cv2.bitwise_and(image, image, mask=mask)

        gray = cv2.cvtColor(masked_image, cv2.COLOR_BGR2GRAY)
        # blur = cv2.GaussianBlur(gray, (5, 5), 0)
        # edges = cv2.Canny(blur, 5, 15, apertureSize=3)

        original_detected_lines = cv2.HoughLinesP(
            gray,  # Input edge image
            1,  # Distance resolution in pixels
            np.pi / s.HoughLinesP_precise_2,  # Angle resolution in radians
            threshold=s.HoughLinesP_threshold,  # Min number of votes for valid line
            minLineLength=s.HoughLinesP_minLineLength_2,  # Min allowed length of line
            maxLineGap=s.HoughLinesP_maxLineGap,  # Max allowed gap between line for joining them
        )

        # print(original_detected_lines)
        # for index, line in enumerate(original_detected_lines):
        #     x1, y1, x2, y2 = line[0]
        #     cv2.line(masked_image, (x1, y1), (x2, y2), (255, 255, 255), 1)
        # cv2.imshow("Detected Lines", masked_image)
        # cv2.waitKey(0)
        # cv2.destroyAllWindows()
        # raise Exception('231')

        # Iterate through the detected lines and draw them on the original image:
        if original_detected_lines is None:
            # @TODO handle exception
            raise Exception("No bearing line detected in image")

        for line in original_detected_lines:
            self.original_lines.append(list(line[0]))

        self.lines_switched_coordinate = alter_coordinate_origin(self.original_lines, image.shape[0])
        # !!@ normalize lines_switched_coordinate
        self.normalize_line_data()
        # !!@ detect track distance
        self.find_target_travel_distance(image_copy)
        # !!@ bearing line related info
        self.update_bl_related_info()
        # @TODO mask line
        self.lines_with_mask = self.lines
        # @TODO display all lines properly
        self.display_lines: t_lines = self.lines
        # for line in self.lines:
        #     x1, y1, x2, y2 = line
        #     if x1 < 0:
        #         x1 = x2
        #         y1 = y2
        #         x2 = self.canvas_size[1]
        #         y2 = calculate_y_coordinate(line, x2)
        #     if y1 < 0:
        #         x1 = x2
        #         y1 = y2
        #         y2 = self.canvas_size[0]
        #         x2 = calculate_y_coordinate(line, y2, find_x=True)
        #     self.display_lines.append([x1, y1, x2, y2])

    # !!@ predict with track distance
    def predict_with_specified_target_track(self, travel_distance: float = 0) -> tuple[float]:
        assert travel_distance > 0
        lines_used = self.lines
        track_fitness_error_collection = {}

        outer_lines_cross_p = find_intersection_point(lines_used[0], lines_used[-1])
        if not outer_lines_cross_p:
            raise Exception("First and last bearring seems parallel")
        try_point = outer_lines_cross_p
        # point on x-axis where line pass it and view_center is vertical to first_line
        x_value, _ = find_intersection_point(self.view_center_group_cross_line, lines_used[0])

        if x_value < outer_lines_cross_p[0]:
            try_step = -s.Try_point_step
        elif x_value > outer_lines_cross_p[0]:
            try_step = s.Try_point_step
        else:
            # @TODO first_line is vertical to x-axis
            raise Exception("predict_target_track error: first_line is vertical to x-axis")

        smallest_error = sys.maxsize
        while cal_perpendicular_distance(try_point, lines_used[-1]) <= travel_distance:
            try_point = (
                try_point[0] + try_step,
                calculate_y_coordinate(lines_used[0], try_point[0] + try_step),
            )
            ok, p1, p2 = find_points_on_line_by_distance(try_point, lines_used[-1], travel_distance)
            if not ok:
                break
            p1_error = cal_track_fitness_metric(try_point, p1, lines_used, self.bl_time_tick)
            p2_error = cal_track_fitness_metric(try_point, p2, lines_used, self.bl_time_tick)
            if min(p1_error, p2_error) > smallest_error:
                break

            if p1_error <= p2_error:
                track_fitness_error_collection[p1_error] = (try_point, p1)
            else:
                track_fitness_error_collection[p2_error] = (try_point, p2)

            smallest_error = min(p1_error, p2_error, smallest_error)

        if not track_fitness_error_collection:
            raise Exception("track_fitness_error compare error!")

        (x1, y1), (x2, y2) = track_fitness_error_collection[smallest_error]
        return (x1, y1, x2, y2, smallest_error)

    def predict_with_target_track(self):
        """find smallest_error using brute force"""
        if not self.target.pd_with_track.track_detected:
            return

        lines_used = self.lines
        track_fitness_error_collection = {}
        travel_distance_used = self.target.pd_with_track.track_distance
        assert travel_distance_used > 0

        outer_lines_cross_p = find_intersection_point(lines_used[0], lines_used[-1])
        if not outer_lines_cross_p:
            raise Exception("First and last bearring seems parallel")
        try_point = outer_lines_cross_p
        # point on x-axis where line pass it and view_center is vertical to first_line
        x_value, _ = find_intersection_point(self.view_center_group_cross_line, lines_used[0])

        if x_value < outer_lines_cross_p[0]:
            try_step = -s.Try_point_step
        elif x_value > outer_lines_cross_p[0]:
            try_step = s.Try_point_step
        else:
            # @TODO first_line is vertical to x-axis
            raise Exception("predict_target_track error: first_line is vertical to x-axis")

        smallest_error = sys.maxsize
        while cal_perpendicular_distance(try_point, lines_used[-1]) <= self.target.pd_with_track.track_distance:
            try_point = (
                try_point[0] + try_step,
                calculate_y_coordinate(lines_used[0], try_point[0] + try_step),
            )
            ok, p1, p2 = find_points_on_line_by_distance(try_point, lines_used[-1], travel_distance_used)
            if not ok:
                break
            p1_error = cal_track_fitness_metric(try_point, p1, lines_used, self.bl_time_tick)
            p2_error = cal_track_fitness_metric(try_point, p2, lines_used, self.bl_time_tick)
            if min(p1_error, p2_error) > smallest_error * 15:
                break

            if p1_error <= p2_error:
                track_fitness_error_collection[p1_error] = (try_point, p1)
            else:
                track_fitness_error_collection[p2_error] = (try_point, p2)

            smallest_error = min(p1_error, p2_error, smallest_error)

        if not track_fitness_error_collection:
            raise Exception("track_fitness_error compare error!")

        (x1, y1), (x2, y2) = track_fitness_error_collection[smallest_error]
        self.target.pd_with_track.track_error = smallest_error
        self.target.pd_with_track.track = [x1, y1, x2, y2]
        self.target.pd_with_track.track_heading = cal_line_angle([x1, y1, x2, y2])
        self.target.pd_with_track.track_distance = travel_distance_used

    def predict_with_target_track_new(self):
        """apply scipy.optimize.minimize() to find smallest_error"""
        if not self.target.pd_with_track.track_detected:
            return

        (
            self.target.pd_with_track.track_error,
            self.target.pd_with_track.track,
            self.target.pd_with_track.track_heading,
        ) = cal_track_info(
            self.target.pd_with_track.track_distance,
            self.lines,
            self.view_center_group_cross_line,
            self.bl_time_tick,
        )

    def predict_with_target_track_new_05(self):
        """test measure error metric by bearing line angle-diff"""
        if not self.target.pd_with_track.track_detected:
            return

        (
            self.target.pd_with_track.track_error,
            self.target.pd_with_track.track,
            self.target.pd_with_track.track_heading,
        ) = cal_track_info_05(
            self.target.pd_with_track.track_distance,
            self.lines,
            self.view_center_group_cross_line,
            self.bl_time_tick,
            self.own_pos,
            self.bl_orientation,
        )

    # !!@ predict without track distance
    def predict_without_target_track_full_brute_force(self, btm_limit: float = 0, top_limit: float = 0):
        self.target.pd_without_track.track_error = 9999
        lines_used = self.lines
        outer_lines_cross_p = find_intersection_point(lines_used[0], lines_used[-1])
        if not outer_lines_cross_p:
            raise Exception("First and last bearring seems parallel")

        if btm_limit == 0 and top_limit == 0:
            btm_limit = s.Travel_distance_min
            top_limit = s.Travel_distance_max

        x_value, _ = find_intersection_point(self.view_center_group_cross_line, lines_used[0])

        if x_value < outer_lines_cross_p[0]:
            try_step = -s.Try_point_step
        elif x_value > outer_lines_cross_p[0]:
            try_step = s.Try_point_step
        else:
            # @TODO first_line is vertical to x-axis
            raise Exception("predict_target_track error: first_line is vertical to x-axis")

        for distance in range(int(btm_limit), int(top_limit), s.Travel_distance_step):
            try_point = outer_lines_cross_p
            track_fitness_error_collection = {}
            travel_distance_used = distance

            smallest_error = sys.maxsize
            while cal_perpendicular_distance(try_point, lines_used[-1]) <= travel_distance_used:
                try_point = (
                    try_point[0] + try_step,
                    calculate_y_coordinate(lines_used[0], try_point[0] + try_step),
                )
                ok, p1, p2 = find_points_on_line_by_distance(try_point, lines_used[-1], travel_distance_used)
                if not ok:
                    break
                p1_error = cal_track_fitness_metric(try_point, p1, lines_used, self.bl_time_tick)
                p2_error = cal_track_fitness_metric(try_point, p2, lines_used, self.bl_time_tick)
                if p1_error <= p2_error:
                    track_fitness_error_collection[p1_error] = (try_point, p1)
                else:
                    track_fitness_error_collection[p2_error] = (try_point, p2)
                smallest_error = min(p1_error, p2_error, smallest_error)

            if not track_fitness_error_collection:
                continue

            (x1, y1), (x2, y2) = track_fitness_error_collection[smallest_error]

            if (
                self.target.pd_without_track.track_error == 0
                or self.target.pd_without_track.track_error > smallest_error
            ):
                self.target.pd_without_track.track_error = smallest_error
                self.target.pd_without_track.track = [x1, y1, x2, y2]
                self.target.pd_without_track.track_heading = cal_line_angle([x1, y1, x2, y2])
                self.target.pd_without_track.track_distance = travel_distance_used

    def predict_without_target_track_partial_brute_force(self, btm_limit: float = 0, top_limit: float = 0):
        error_limit = 500

        lines_used = self.lines
        outer_lines_cross_p = find_intersection_point(lines_used[0], lines_used[-1])
        if not outer_lines_cross_p:
            raise Exception("First and last bearring seems parallel")

        if btm_limit == 0 and top_limit == 0:
            btm_limit = s.Travel_distance_min
            top_limit = s.Travel_distance_max

        x_value, _ = find_intersection_point(self.view_center_group_cross_line, lines_used[0])

        if x_value < outer_lines_cross_p[0]:
            try_step = -s.Try_point_step
        elif x_value > outer_lines_cross_p[0]:
            try_step = s.Try_point_step
        else:
            # @TODO first_line is vertical to x-axis
            raise Exception("predict_target_track error: first_line is vertical to x-axis")

        distance_fit_error_collection = {}
        for travel_distance_used in range(int(btm_limit), int(top_limit), s.Travel_distance_step):
            try_point = outer_lines_cross_p

            track_fitness_error_collection = {}
            smallest_error = sys.maxsize
            while cal_perpendicular_distance(try_point, lines_used[-1]) <= travel_distance_used:
                try_point = (
                    try_point[0] + try_step,
                    calculate_y_coordinate(lines_used[0], try_point[0] + try_step),
                )
                ok, p1, p2 = find_points_on_line_by_distance(try_point, lines_used[-1], travel_distance_used)
                if not ok:
                    break
                p1_error = cal_track_fitness_metric(try_point, p1, lines_used, self.bl_time_tick)
                p2_error = cal_track_fitness_metric(try_point, p2, lines_used, self.bl_time_tick)
                if min(p1_error, p2_error) > smallest_error * 15:
                    break
                if p1_error <= p2_error:
                    track_fitness_error_collection[p1_error] = (try_point, p1)
                else:
                    track_fitness_error_collection[p2_error] = (try_point, p2)
                smallest_error = min(p1_error, p2_error, smallest_error)

            if not track_fitness_error_collection:
                continue

            if (
                len(distance_fit_error_collection) > 1
                and smallest_error > min(distance_fit_error_collection.keys()) * error_limit
            ):
                break

            (x1, y1), (x2, y2) = track_fitness_error_collection[smallest_error]
            distance_fit_error_collection[smallest_error] = [
                [x1, y1, x2, y2],
                travel_distance_used,
            ]

        smallest_error = min(distance_fit_error_collection.keys())
        best_distance = distance_fit_error_collection[smallest_error][1]
        x1, y1, x2, y2 = distance_fit_error_collection[smallest_error][0]
        self.target.pd_without_track.track = [x1, y1, x2, y2]
        self.target.pd_without_track.track_heading = cal_line_angle([x1, y1, x2, y2])
        self.target.pd_without_track.track_distance = best_distance
        self.target.pd_with_track.track_error = smallest_error

    def predict_without_target_track_new_02(self, btm_limit: float = 0, top_limit: float = 0):
        """find the best track using two stage strategy"""
        error_limit = 500

        self.target.pd_without_track.track_error = 9999
        lines_used = self.lines
        outer_lines_cross_p = find_intersection_point(lines_used[0], lines_used[-1])
        if not outer_lines_cross_p:
            raise Exception("First and last bearring seems parallel")

        if btm_limit == 0 and top_limit == 0:
            btm_limit = s.Travel_distance_min_first
            top_limit = s.Travel_distance_max_first

        x_value, _ = find_intersection_point(self.view_center_group_cross_line, lines_used[0])

        if x_value < outer_lines_cross_p[0]:
            try_step = -s.Try_point_step_first
        elif x_value > outer_lines_cross_p[0]:
            try_step = s.Try_point_step_first
        else:
            # @TODO first_line is vertical to x-axis
            raise Exception("predict_target_track error: first_line is vertical to x-axis")

        distance_fit_error_collection = {}
        for travel_distance_used in range(int(btm_limit), int(top_limit), s.Travel_distance_step_first):
            try_point = outer_lines_cross_p
            try_point_error_collection = []
            smallest_error = sys.maxsize
            while cal_perpendicular_distance(try_point, lines_used[-1]) <= travel_distance_used:
                try_point = (
                    try_point[0] + try_step,
                    calculate_y_coordinate(lines_used[0], try_point[0] + try_step),
                )
                ok, p1, p2 = find_points_on_line_by_distance(try_point, lines_used[-1], travel_distance_used)
                if not ok:
                    break
                p1_error = cal_track_fitness_metric(try_point, p1, lines_used, self.bl_time_tick)
                p2_error = cal_track_fitness_metric(try_point, p2, lines_used, self.bl_time_tick)
                if min(p1_error, p2_error) > smallest_error * 15:
                    break
                if p1_error <= p2_error:
                    try_point_error_collection.append(p1_error)
                else:
                    try_point_error_collection.append(p2_error)
                smallest_error = min(p1_error, p2_error, smallest_error)

            if not try_point_error_collection:
                continue

            if (
                len(distance_fit_error_collection) > 1
                and smallest_error > min(distance_fit_error_collection.keys()) * error_limit
            ):
                break

            distance_fit_error_collection[smallest_error] = travel_distance_used

        first_stage_smallest_error = min(distance_fit_error_collection.keys())
        first_stage_best_distance = distance_fit_error_collection[first_stage_smallest_error]
        # print(first_stage_best_distance, first_stage_smallest_error)
        self.predict_without_target_track_partial_brute_force(
            first_stage_best_distance - 3 * s.Travel_distance_step_first,
            first_stage_best_distance + 3 * s.Travel_distance_step_first,
        )
        # print("-" * 44)
        # print(self.target.pd_without_track.track_distance)
        # print(self.target.pd_with_track.track_distance)

    def predict_without_target_track_new_04(self, btm_limit: float = 0, top_limit: float = 0):
        """find the best track using two stage strategy and minimize()"""
        if btm_limit == 0 and top_limit == 0:
            btm_limit = s.Travel_distance_min_first
            top_limit = s.Travel_distance_max_first

        smallest_error = 99999
        first_stage_best_distance = 500
        for distance in range(m.ceil(btm_limit), m.floor(top_limit), s.Travel_distance_step_first):
            track_error = evaluate_track(
                distance,
                self.lines,
                self.view_center_group_cross_line,
                self.bl_time_tick,
            )[0]
            if smallest_error > track_error:
                smallest_error = track_error
                first_stage_best_distance = distance

        self.predict_without_target_track_partial_brute_force(
            first_stage_best_distance - 2 * s.Travel_distance_step_first,
            first_stage_best_distance + 2 * s.Travel_distance_step_first,
        )

    def predict_without_target_track_new_05(self, btm_limit: float = 0, top_limit: float = 0):
        """use bearing line angle-diff metric"""
        if btm_limit == 0 and top_limit == 0:
            btm_limit = s.Travel_distance_min_first
            top_limit = s.Travel_distance_max_first

        smallest_error = 99999
        first_stage_best_distance = 500
        for distance in range(m.ceil(btm_limit), m.floor(top_limit), s.Travel_distance_step_first):
            track_error = evaluate_track_05(
                distance,
                self.lines,
                self.view_center_group_cross_line,
                self.bl_time_tick,
                self.own_pos,
                self.bl_orientation,
            )[0]
            if smallest_error > track_error:
                smallest_error = track_error
                first_stage_best_distance = distance

        self.predict_without_target_track_partial_brute_force(
            first_stage_best_distance - 2 * s.Travel_distance_step_first,
            first_stage_best_distance + 2 * s.Travel_distance_step_first,
        )
        # print("-" * 44)
        # print(self.target.pd_without_track.track_distance)
        # print(self.target.pd_with_track.track_distance)

    def predict_without_target_track_new_06(self, btm_limit: float = 0, top_limit: float = 0):
        """using postion-diff metric and minimize() to find the best track distance"""
        if btm_limit == 0 and top_limit == 0:
            btm_limit = s.Travel_distance_min_first
            top_limit = s.Travel_distance_max_first
        bounds = [(btm_limit, top_limit)]
        # bounds = [(min(try_x_bottom, try_x_top), max(try_x_bottom, try_x_top)), (None, None)]
        predict_06(bounds, self.lines, self.view_center_group_cross_line, self.bl_time_tick)

    def predict_BFGS(self, btm_limit: float = 0, top_limit: float = 0):
        """find the best track using minimize() and BFGS"""
        if btm_limit == 0 and top_limit == 0:
            btm_limit = s.Travel_distance_min_first
            top_limit = s.Travel_distance_max_first

        # ok, x, y, z = bfgs_predict(self.own_pos, self.bl_orientation, self.bl_time_tick)
        t = bfgs_predict(self.own_pos, self.bl_orientation, self.bl_time_tick)
        print("-" * 55)
        print(t)


if __name__ == "__main__":
    print("Do not run this script directly")
